#include "JCSearchMatcherI.h"
#include "Point.h"
#include <vector>
#include "AssociationSet.h"
#include "PlaneAssociation.h"
#include <queue>

using namespace std;

JCSearchMatcherI::JCSearchMatcherI()
{
	name = "JCSearchMatcherI";
	printf("new JCSearchMatcherI\n");
}

JCSearchMatcherI::~JCSearchMatcherI()
{
	printf("delete JCMergeMatcherI\n");
}

void JCSearchMatcherI::update(){}

class AssoSet
{
	public:
	AssociationSet * set;
	AssoSet();
	~AssoSet();
};

AssoSet::AssoSet(){}
AssoSet::~AssoSet(){}

bool operator< (const AssoSet& a, const AssoSet &b)
{
	return a.set->val > b.set->val;	
}

bool operator> (const AssoSet& a, const AssoSet &b)
{
	return a.set->val < b.set->val;	
}

using namespace std;

const bool debugg_JCSearchMatcherI = true;
Transformation * JCSearchMatcherI::getTransformation(RGBDFrame * src, RGBDFrame * dst)
{
	if(debugg_JCSearchMatcherI){printf("JCSearchMatcherI::getTransformation(%i,%i)\n",src->id,dst->id);}
	
	IplImage* img_combine;
	int width;
	int height;
	if(debugg_JCSearchMatcherI)
	{	
		width = src->width;
		height = src->height;
		
		img_combine = cvCreateImage(cvSize(2*width,height), IPL_DEPTH_32F, 3);
		float * data = (float *)img_combine->imageData;
		int index = 0;
		for (int j = 0; j < height; j++)
		{
			for (int i = 0; i < width; i++)
			{
				data[3 * (j * (2*width) + (width+i)) + 0] = float(dst->cloud->points[index].b)/255.0f;
				data[3 * (j * (2*width) + (width+i)) + 1] = float(dst->cloud->points[index].g)/255.0f;
				data[3 * (j * (2*width) + (width+i)) + 2] = float(dst->cloud->points[index].r)/255.0f;
				index++;
			}
		}
		index = 0;
		for (int j = 0; j < height; j++)
		{
			for (int i = 0; i < width; i++)
			{
				data[3 * (j * (2*width) + (i)) + 0] = float(src->cloud->points[index].b)/255.0f;
				data[3 * (j * (2*width) + (i)) + 1] = float(src->cloud->points[index].g)/255.0f;
				data[3 * (j * (2*width) + (i)) + 2] = float(src->cloud->points[index].r)/255.0f;
				index++;
			}
		}
		
		for(unsigned int i = 0; i < src->keypoints->valid_key_points.size(); i++){
			cvCircle(img_combine,cvPoint(src->keypoints->valid_key_points.at(i)->point->w, src->keypoints->valid_key_points.at(i)->point->h), 3,cvScalar(0, 0, 255, 0),1, 8, 0);
			cvCircle(img_combine,cvPoint(src->keypoints->valid_key_points.at(i)->point->w, src->keypoints->valid_key_points.at(i)->point->h), 4,cvScalar(0, 255, 0, 0),1, 8, 0);
		}
		
		for(unsigned int i = 0; i < dst->keypoints->valid_key_points.size(); i++){
			cvCircle(img_combine,cvPoint(dst->keypoints->valid_key_points.at(i)->point->w + width, dst->keypoints->valid_key_points.at(i)->point->h), 3,cvScalar(0, 0, 255, 0),1, 8, 0);
			cvCircle(img_combine,cvPoint(dst->keypoints->valid_key_points.at(i)->point->w + width, dst->keypoints->valid_key_points.at(i)->point->h), 4,cvScalar(0, 255, 0, 0),1, 8, 0);
		}
	}
	AssociationSet * set = new AssociationSet();
	float plane_limit = 0.01;
	
	for(unsigned int i = 0; i < src->planes->size(); i++){
		Plane * src_plane = src->planes->at(i);
		for(unsigned int j = 0; j < dst->planes->size(); j++){
			Plane * dst_plane = dst->planes->at(j);
			float dist = src_plane->color_histogram->distance(dst_plane->color_histogram)*0.5f;
			if(dist < plane_limit){
				PlaneAssociation * pa = new PlaneAssociation(src_plane,dst_plane, dist, plane_limit);
				set->possible_associations->push_back(pa);
				set->possible_associations->back()->id = set->possible_associations->size()-1;
			}
		}
	}
	
	unsigned int nr_valid_src = src->keypoints->valid_key_points.size();
	unsigned int nr_valid_dst = dst->keypoints->valid_key_points.size();
	float limit = 0.05f;
	for(unsigned int i = 0; i < nr_valid_src; i++){
		KeyPoint * src_kp = src->keypoints->valid_key_points.at(i);
		for(unsigned int j = 0; j < nr_valid_dst; j++){
			KeyPoint * dst_kp = dst->keypoints->valid_key_points.at(j);
			float dist = src_kp->descriptor->distance(dst_kp->descriptor);
			if(dist < limit){
				set->possible_associations->push_back(new KeyPointAssociation(src_kp , dst_kp, dist, limit));
				set->possible_associations->back()->id = set->possible_associations->size()-1;
			}
		}
	}
	
	//printf("set->possible_associations.size() = %i\n",set->possible_associations->size());
	set->bestcase = set->best_p();
	int len  = set->possible_associations->size();
	set->len = set->possible_associations->size();
	set->bestcase = -999999999999999999;
	set->hash = new char[1+len/8];
	for(int i = 0; i < 1+len/8; i++){set->hash[i] = 0;}
	
	const int hashmap_size = 1024;
	std::vector<char *> ** hashmap = new std::vector<char *>*[hashmap_size];
	for(int hm = 0; hm < hashmap_size; hm++){hashmap[hm] = new std::vector<char *>();}
	
	priority_queue<AssoSet> queue;
	AssoSet set_val;
	set_val.set = set;
	set->update_possible();
	queue.push(set_val);
	//float best_found = 0;
	bool stop = false;
	int counter = 1000;
	//exit(0);
	while(!queue.empty() && counter > 0 && !stop)
	{
		counter--;
		AssoSet top = queue.top();
		queue.pop();
		std::vector<AssociationSet * > * exploration = top.set->explore();
		AssociationSet * current_set = top.set;
		printf("counter: %i, val: %f\n",counter,current_set->val);

		IplImage * img_combine_clone = cvCreateImage(cvSize(img_combine->width, img_combine->height), IPL_DEPTH_32F, 3);
		cvCopy( img_combine, img_combine_clone, NULL );
		float * clone_data = (float *)img_combine_clone->imageData;
		for(unsigned int i = 0; i < current_set->selected_associations->size(); i++)
		{
			Association * asso = current_set->selected_associations->at(i);
			float r = float(rand()%10000)/10000.0f;
			float g = float(rand()%10000)/10000.0f;
			float b = float(rand()%10000)/10000.0f;
			if(asso->type == plane_association){
				PlaneAssociation * a1 = (PlaneAssociation *)asso;
				Plane * src_plane = a1->p1;
				Plane * dst_plane = a1->p2;
				for(unsigned int k = 0; k < dst_plane->p_w.size(); k++){
					int w = dst_plane->p_w.at(k);
					int h = dst_plane->p_h.at(k);
					clone_data[3 * (h * (2*width) + (width+w)) + 0] = r;
					clone_data[3 * (h * (2*width) + (width+w)) + 1] = g;
					clone_data[3 * (h * (2*width) + (width+w)) + 2] = b;
				}
			
				for(unsigned int k = 0; k < src_plane->p_w.size(); k++){
					int w = src_plane->p_w.at(k);
					int h = src_plane->p_h.at(k);
					clone_data[3 * (h * (2*width) + (w)) + 0] = r;
					clone_data[3 * (h * (2*width) + (w)) + 1] = g;
					clone_data[3 * (h * (2*width) + (w)) + 2] = b;
				}
			}
		}
		for(unsigned int i = 0; i < current_set->selected_associations->size(); i++)
		{
			Association * asso = current_set->selected_associations->at(i);
			if(asso->type == keypoint_association){
				KeyPointAssociation * a1 = (KeyPointAssociation *)asso;
			
				Point * pa_src = a1->p1->point;
				Point * pa_dst = a1->p2->point;
				cvLine  (img_combine_clone,cvPoint(pa_src->w 		 , pa_src->h),cvPoint(pa_dst->w + width , pa_dst->h),cvScalar(0, 0,255, 0),1, 8, 0);
			}
		}
		cvNamedWindow("combined image clone", CV_WINDOW_AUTOSIZE );
		cvShowImage("combined image clone", img_combine_clone);
		cvWaitKey(0);
		cvReleaseImage( &img_combine_clone );

		for(unsigned int i = 0; i < exploration->size(); i++)
		{
			
			AssociationSet * insp = exploration->at(i);
			char * hash = insp->getHashValue();
			
			int sum = 0;
			for(int j = 0; j < 1+len/8; j++)
			{
				if(hash[j] < 0)	{sum -= hash[j];}
				else			{sum += hash[j];}
			}
			bool in_hashmap = false;
			sum = sum % hashmap_size;
			for(unsigned int j = 0; j < hashmap[sum]->size();j++)
			{
				bool same = true;
				char * hash2 = hashmap[sum]->at(j);
				for(int k = 0; k < 1+len/8; k++){same = same && (hash[k] == hash2[k]);}
				if(same){
					in_hashmap = true;
					break;
				}
			}
			if(!in_hashmap)
			{
				insp->update_possible();
				hashmap[sum]->push_back(hash);
				
				AssoSet * q_val_ptr = new AssoSet();
				q_val_ptr->set = insp;
				
				AssoSet q_val = *q_val_ptr;				
				queue.push(q_val);
			}else{
				delete insp;
			}
		}
		delete exploration;
	}
	for(int hm = 0; hm < hashmap_size; hm++){
		delete hashmap[hm];
	}
	delete[] hashmap;
	delete set;
	
	if(debugg_JCSearchMatcherI)
	{
		cvNamedWindow("combined image", CV_WINDOW_AUTOSIZE );
		cvShowImage("combined image", img_combine);
		cvWaitKey(0);
		cvReleaseImage( &img_combine );
	}
	
	Transformation * transformation = new Transformation();
	transformation->src = src;
	transformation->dst = dst;
	transformation->weight = 100;
	transformation->level = 1;
	return transformation;
}
